home *** CD-ROM | disk | FTP | other *** search
/ El Mac 9 / El Mac 9.iso / Shareware / Applications / MathPad 2.4 / Examples / incl / rotations < prev    next >
Encoding:
Text File  |  1995-04-30  |  1.7 KB  |  44 lines  |  [TEXT/MPad]
  1. -- Rotations
  2. -- by Hank.Dolben@UNH.edu 1994 May 18
  3. ----------------------------------------------------------------------------
  4. -- a is a vector in coordinate system A.
  5. -- a' is the vector in coordinate system A'.
  6. -- A' is rotated by yaw around z, pitch around y, and roll around x, from A.
  7. -- R = rotation(yaw,pitch,roll)
  8. -- a' = Ra = transform(R,a)
  9. -- the rows of R are the basis vectors of A' in A.
  10. ----------------------------------------------------------------------------
  11. rotation(y,p,r) = {{cos(p)*       cos(y),
  12.                     cos(p)*       sin(y),
  13.                    -sin(p)                            },
  14.                    {sin(p)*sin(r)*cos(y)-cos(r)*sin(y),
  15.                     sin(p)*sin(r)*sin(y)+cos(r)*cos(y),
  16.                     cos(p)*sin(r)                     },
  17.                    {sin(p)*cos(r)*cos(y)+sin(r)*sin(y),
  18.                     sin(p)*cos(r)*sin(y)-sin(r)*cos(y),
  19.                     cos(p)*cos(r)                     }}
  20.  
  21. -- when |pitch| = 90 (cos(pitch)=0), the partition of yaw and roll is arbitrary,
  22. -- say roll = 0, then:
  23. --   R[2,1] = sin(p)*sin(r)*cos(y)-cos(r)*sin(y) = -sin(y)
  24. --   R[2,2] = sin(p)*sin(r)*sin(y)+cos(r)*cos(y) =  cos(y)
  25.  
  26. Yaw(R)   = atan2(-R[2,1],R[2,2]) when R[1,2]=0 and R[1,1]=0,
  27.            atan2(R[1,2],R[1,1]) otherwise
  28.  
  29. Pitch(R) = asin(-R[1,3])        --   -90 ≤ Pitch(R) ≤ 90
  30.  
  31. Roll(R)  = 0 when R[2,3]=0 and R[3,3]=0,
  32.            atan2(R[2,3],R[3,3]) otherwise
  33.  
  34. rotate(yaw,pitch,roll,v) = transform(rotation(yaw,pitch,roll),v)
  35.  
  36. transform(R,b)[i] = dot(R[i],b) dim [count(R)]
  37.  
  38. -- compose the transformations such that R2 applies after R1
  39. multiply(R2,R1)[i,j] = dot(R2[i],transpose(R1)[j]) dim [count(R2),count(R1[1])]
  40.  
  41. transpose(R)[i,j] = R[j,i] dim [count(R[1]),count(R)]
  42.  
  43. dot(a,b) = sum(a[i]*b[i],i,1,count(a))
  44.